#include "cybertron/glm/function.hpp"
#include "cybertron/gis/GIS.hpp"
#include "cybertron/gis/ECEF.hpp"

CYBERTRON_BEGIN

CybertronCoreAPI Deg2UTM GIS::wgsToUTM(double longitude, double latitude)
{
	return Deg2UTM(latitude, longitude);
}

CybertronCoreAPI Deg2UTM GIS::wgsToUTM(double longitude, double latitude, int zone)
{
	return Deg2UTM(latitude, longitude, zone);
}

CybertronCoreAPI Deg2UTM GIS::wgsToUTM(const dvec2& lonLat)
{
	return Deg2UTM(lonLat.y, lonLat.x);
}

CybertronCoreAPI Deg2UTM GIS::wgsToUTM(const dvec3& lonLatAlt)
{
	Deg2UTM utm(lonLatAlt.y, lonLatAlt.x);
	utm.WgsHeight = lonLatAlt.z;

	return utm;
}

CybertronCoreAPI dvec3 GIS::utmToWGS(const Deg2UTM& utm)
{
	UTM2Deg deg;
	deg.build(utm);

	return dvec3(deg.Longitude, deg.Latitude, deg.Altitude);
}

CybertronCoreAPI dvec3 GIS::wgsToEcef(double longitude, double latitude, double altitude)
{
	latitude = degreeToRadian(latitude);
	longitude = degreeToRadian(longitude);

	return ECEF::fromGeodetic(dvec3(latitude, longitude, altitude));
}

CybertronCoreAPI dvec3 GIS::wgsToEcef(const dvec3& lonLatAlt)
{
	return wgsToEcef(lonLatAlt.x, lonLatAlt.y, lonLatAlt.z);
}

CybertronCoreAPI dvec3 GIS::wgsToEcef(const dvec2& lonLat)
{
	return wgsToEcef(lonLat.x, lonLat.y, 0);
}

CybertronCoreAPI dvec3 GIS::ecefToWGS(const dvec3& ecef)
{
	dvec3 geodetic = ECEF::toGeodetic(ecef);

	return dvec3(
		radianToDegree(geodetic.y),
		radianToDegree(geodetic.x),
		geodetic.z);
}

CybertronCoreAPI CartesianCoordinateSystem GIS::buildLocalEnu(const dvec3& ecef)
{
	dvec3 wgs = ecefToWGS(ecef);
	dvec3 vnorth = wgsToEcef(wgs.x, wgs.y + 1, wgs.z);

	dvec3 axisZ = normalize(ecef);
	dvec3 axisX = normalize(cross(vnorth, ecef));
	dvec3 axisY = normalize(cross(axisZ, axisX));

	CartesianCoordinateSystem result;
	result.setCenter(ecef);
	result.setAxisX(axisX);
	result.setAxisY(axisY);
	result.setAxisZ(axisZ);

	return result;
}

CYBERTRON_END
